/*
 * Kinect Player
 * Kulikov Victor
 * kulikov.victor@gmail.com
 * GNU Lisence 2011
 */
#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <QMessageBox>
#include <QFileDialog>
#include <libfreenect_sync.h>
#include <libfreenect.h>
#include <QDebug>
#include <Kinect/kinectframe.h>
#include <QTextStream>
#include <Camera/calibration.h>
#include <calibrationdialog.h>

#include <string>
using namespace cv;
using namespace kinect;
using namespace camera;

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)

{
    grabFrame(0);
    xtimer.setInterval(2000);
    connect(&xtimer,SIGNAL(timeout()),this,SLOT(tryCallib()));
    ui->setupUi(this);
    storage = 0;
    timer = new QTimer(this);
    rtimer = new QTimer(this);

    //Connecting controls buttons
    connect(ui->play,SIGNAL(seeked(qlonglong)),this,SLOT(onSeek(qlonglong))); 
    connect(this,SIGNAL(sendFrames(KinectFrame)),ui->view,SLOT(reciveFrame(KinectFrame)));
    connect(this,SIGNAL(sendFrames(KinectFrame)),this,SLOT(currentFrameChanged(KinectFrame)));
    connect(ui->play,SIGNAL(sig_play()),timer,SLOT(start()));
    connect(ui->play,SIGNAL(sig_pause()),timer,SLOT(stop()));
    connect(ui->play,SIGNAL(stop()),timer,SLOT(stop()));
    connect(timer,SIGNAL(timeout()),ui->play,SLOT(next()));
    connect(timer,SIGNAL(timeout()),this,SLOT(onTimer()));
    connect(rtimer,SIGNAL(timeout()),this,SLOT(grabPicture()));

    timer->setInterval(1000/25);
    rtimer->setInterval(1000/25);

    //calib.setCallibrationWidget(&cr);
    //cr.show();
 //   openFile("test.kin");
}
void MainWindow::onTimer()
{
    if(storage)
    {
        KinectFrame frame=grabFrame(0);
      //  storage->read();
        emit sendFrames(frame);
    }
}

bool MainWindow::openFile(std::string fname)
{
    if(storage)
    {
        delete storage;
        storage = 0;
    }
    try{
        storage = new KinectStorage(fname,KinectStorage::READ);
        timer->setInterval(1000/storage->getFps());
        ui->play->setLength(storage->length());
        ui->play->setRate(storage->getFps());
        KinectFrame* frame=storage->read(0);
        if(!frame)
            return false;
        emit sendFrames(*frame);
    }catch(...)
    {
        QMessageBox::warning(this,"Error","Old or unsupported format.");
        delete storage;
        storage = 0;
        return false;
    }

    return true;

}

MainWindow::~MainWindow()
{
    freenect_sync_stop();
    if(storage)
    {
        delete storage;
        storage = 0;
    }
    delete ui;
}

void MainWindow::changeEvent(QEvent *e)
{
    QMainWindow::changeEvent(e);
    switch (e->type()) {
    case QEvent::LanguageChange:
        ui->retranslateUi(this);
        break;
    default:
        break;
    }
}
void MainWindow::onSeek(qlonglong x)
{
    if(storage)
    {
        KinectFrame* frame=storage->read((long)x);


        emit sendFrames(*frame);
    }
}


void MainWindow::on_actionClose_triggered()
{
    this->close();
}

void MainWindow::on_actionAbout_triggered()
{
    QMessageBox::about(0,"About Kinect Player","Project started by\nKulikov Victor\nkulikov.victor@gmail.com\nhttp://victoropencv.blogspot.com\nUnder GNU lisence 2011");
}

void MainWindow::on_actionControls_triggered()
{
    on_actionTest_triggered();
   // QMessageBox::about(0,"Kinect Player Help","Use mouse wheel to move inside 3D picture.\nRecord saves kinnect data intofile.\nDialog to open kinect file.");
}

void MainWindow::on_actionOpen_triggered()
{
    QFileDialog dialog;
    QString fname =dialog.getOpenFileName(this,"Open Kinect File","","*.kin");
    if(!fname.isEmpty())
    {
        openFile(fname.toStdString());
    }
}

void MainWindow::on_actionRecord_triggered()
{

    if(!storename.isEmpty())
    {

        rtimer->stop();
        freenect_sync_stop();
        if(storage)
        {
           // storage->close();
            delete storage;
            storage = 0;
        }
        ui->play->setDisabled(false);

        openFile(storename.toStdString());
        storename="";
        return;
    }
    QFileDialog dialog;
    QString fname =dialog.getSaveFileName(this,"Select File to Save","","*.kin");
    if(!fname.isEmpty())
    {
        storename = fname;
        ui->play->setDisabled(true);



        if(storage)
        {
            delete storage;
            storage = 0;
        }
        storage = new KinectStorage(fname.toStdString(),KinectStorage::WRITE);

        rtimer->start();
    }
}
void MainWindow::stopRecord()
{
    storename="";
    rtimer->stop();
    freenect_sync_stop();
    if(storage)
    {
        storage->close();
        delete storage;
        storage = 0;
    }
    ui->play->setDisabled(false);
    openFile(storename.toStdString());

}

void MainWindow::grabPicture()
{
     if(storage)
     {
         KinectFrame kf=grabFrame(0);

         emit sendFrames(kf);
         std::vector<double> accel;
         accel.push_back(kf.accel().x);
         accel.push_back(kf.accel().y);
         accel.push_back(kf.accel().z);
         storage->write(&kf);
     }
}
void MainWindow::currentFrameChanged(KinectFrame frame)
{
    current = frame;
}

void MainWindow::on_actionSave_Point_Cloud_triggered()
{

    QFileDialog dialog;
    QString fname =dialog.getSaveFileName(this,"Select File to Save","","*.txt");
    if(!fname.isEmpty())
    {
        QFile data(fname);
        if (data.open(QFile::WriteOnly | QFile::Truncate))
        {
           QTextStream out(&data);

           std::vector<Point3dRGB> x = ks.transform(this->grabFrame(0));
           out << x.size() << '\n';
           for(unsigned int i=0;i<x.size();i++)
           {
               out << x[i].x << " " << x[i].y << " " << x[i].z << " " << x[i].r << " " << x[i].g << " " << x[i].b << "\n";
           }
        }
    }
}

KinectFrame  MainWindow::grabFrame(int id)
{
    Mat rgb,depth;
    //Buffers init
    rgb.create(cv::Size(FREENECT_FRAME_W,FREENECT_FRAME_H),CV_8UC3);
    depth.create(cv::Size(FREENECT_FRAME_W,FREENECT_FRAME_H),CV_16UC1);
  //  Mat tmp(cv::Size(FREENECT_IR_FRAME_W,FREENECT_IR_FRAME_H),CV_16UC1);

    uint32_t ts;
    char *t_rgb = 0;
    short *t_depth=0;
    double dx,dy,dz;

    freenect_sync_get_depth((void**)&t_depth, &ts, id, FREENECT_DEPTH_11BIT);
    freenect_sync_get_video((void**)&t_rgb,&ts,id,FREENECT_VIDEO_RGB);

    if(0==t_rgb||0==t_depth)
    {
        return KinectFrame();
    }

    freenect_raw_tilt_state* state;

    freenect_sync_get_tilt_state(&state,id);
    freenect_get_mks_accel(state,&dx,&dy,&dz);
    cv::Point3f pos = cv::Point3f(dx,dy,dz);
    memcpy(rgb.data,t_rgb,FREENECT_VIDEO_RGB_SIZE);
    memcpy(depth.data,t_depth,FREENECT_DEPTH_11BIT_SIZE);
    return KinectFrame(id,rgb,depth,pos);
}

void MainWindow::on_actionCalibrate_triggered()
{
    CalibrationDialog cd;
    cd.exec();
   /* if(calib.callibrate())
    {
        qDebug() << "Calibrated!!!";
        calib.saveSettings("ololo.xml");
    }else{
        calib.loadSettings("ololo.xml");
    }

        KinectFrame kf=grabFrame(0);
        ks.assignDeviceCallibration(0,calib);
        ks.transform(kf);*/
}

void MainWindow::tryCallib()
{

    static int x=0;
    if(7<=x)
    {
       qDebug() << "Stop by max pictures";
       xtimer.stop();
       x=0;
    }
    char* rgb=0;
    char* ir=0;
    uint32_t ts,ts2;
    cv::Mat rgbMat;
    cv::Mat irMat;
    irMat.create(cv::Size(FREENECT_IR_FRAME_W,FREENECT_IR_FRAME_H),CV_8UC1);
    rgbMat.create(cv::Size(FREENECT_FRAME_W,FREENECT_FRAME_H),CV_8UC3);


    qDebug() << "Get IR";
    freenect_sync_get_video((void**)&ir,  &ts, 0, FREENECT_VIDEO_IR_8BIT);
    qDebug() << "IR memcpy";
    memcpy(irMat.data,ir,FREENECT_VIDEO_IR_8BIT_SIZE);
    qDebug() << "Get RGB";

    freenect_sync_get_video((void**)&rgb,  &ts2, 0, FREENECT_VIDEO_RGB);
    qDebug() << "Time lapce=" << ts2-ts;
    memcpy(rgbMat.data,rgb,FREENECT_VIDEO_RGB_SIZE);

    if(calib.addImage(irMat,rgbMat)){
        qDebug() << "Success";
        x++;
    }else{
        qDebug() << "Fail";
    }
}

void MainWindow::on_actionTest_triggered()
{
    tryCallib();
    /*if(xtimer.isActive())
    {
        xtimer.stop();
    }else{
        xtimer.start();
    }*/
}
